﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Robotics.Race
{
    public class WheelController : Controller
    {
        private int inverted = 1;
        private bool wasInverted = false;
		public WheelController()
		{
		}

        public override void Drive(SensedEnvironment environment, ControlGear gear)
        {
            DirectInputWrapper.Poll();
            gear.Acceleration = -(double)DirectInputWrapper.Y * inverted / 3500;
            gear.Steering = (double)DirectInputWrapper.X / 4000;

            gear.Handbrake = false;
            for (int i = 0; i < 2; ++i)
            {
                gear.Handbrake = gear.Handbrake || DirectInputWrapper.Button[i] > 0;
            }

            bool nowInverted = false;
            for (int i = 2; i < 6; ++i)
            {
                if (DirectInputWrapper.Button[i] > 0)
                {
                    if (!wasInverted)
                        inverted *= -1;
                    nowInverted = true;
                }
            }
            if (!nowInverted)
                wasInverted = false;
            if (environment.Speed < 0 && gear.Acceleration < 0 && inverted > 0)
                gear.Handbrake = true;
            else if (environment.Speed > 0 && gear.Acceleration > 0 && inverted < 0)
                gear.Handbrake = true;
        }
    }
}
